#ifndef OOPSMP_UTILS_GEOMETRY_JOINTS_JOINTS3DTAIL_H_
#define OOPSMP_UTILS_GEOMETRY_JOINTS_JOINTS3DTAIL_H_

#include "OOPSMP/Utils/Geometry/Joints/Joints3D.h"

namespace OOPSMP {
	
	/**
	 *@brief Macro for class, class pointer, and const class pointer
	 *       declarations 
	 */
	ForwardClassDeclaration(Joint3DTail);
	
	/**
	 *@brief joint 3d tail
	 */
	class Joint3DTail : public Joints3D {
		
	public:
		
		/**
		 *@brief Default constructor
		 */
		Joint3DTail() {
			jointLimitsMin = new double[7];
			jointLimitsMax = new double[7];
		}
		
		/**
		 *@brief Default destructor
		 */
		~Joint3DTail() {
			delete [] jointLimitsMin;
			delete [] jointLimitsMax;
		}
		
		/**
		 *@brief Sets joint range of values
		 *@param min array of minimum values
		 *@param max array of maximum values
		 */
		void setJointLimits( const double * min, const double * max ) { }
		
		/**
		 *@brief Returns joint range of values
		 *@param min empty array for minimum values
		 *@param max empty array for maximum values
		 */
		void getJointLimits( double * min, double * max ) const { }
		
		/**
		 *@brief Returns number of degrees of freedom for joint
		 */
		int getNumDoFs() const { return 7; }
		
		/**
		 *@brief Returns true iff degree of freedom represents angle
		 *@param dof_id index of degree of freedom in this joint
		 */
		bool isDoFAngle( int dof_id ) const { return ( dof_id < 3 ) ? false : true; }

		/**
		 *@brief Returns transformation between two robot links
		 *@param link1_dims dimensions of link we transform from
		 *@param link2_dims dimensions of link we transform to
		 *@values values of dofs for joint connecting links
		 *@param result empty array to contain transformation
		 */
		void getTransformationMatrix( double *link1_dims, double *link2_dims, const double * values, double * result ) const
		{	
			SE3Topology::getSingleton()->mapGroupToMatrix(values, result);
		}

	};
	
	/**
     *@brief Declare factory and specify in source file the functions that should
     *       be registered for plug-and-play functionality
     */
	DeclareInstanceFactory(Joint3DTail, Joints3DFactory);

}

#endif